import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import Command
from launch_ros.actions import Node

def generate_launch_description():
    # 获取包共享目录
    bringup_dir = get_package_share_directory('rm_bringup')
    robot_description_dir = get_package_share_directory('rm_robot_description')

    # 加载 launch 参数
    launch_params_path = os.path.join(bringup_dir, 'config', 'launch_params.yaml')
    with open(launch_params_path, 'r') as file:
        launch_params = yaml.safe_load(file)

    # 加载 robot_gimbal_description
    robot_gimbal_description = Command(['xacro ', os.path.join(
        robot_description_dir, 'urdf', 'rm_gimbal.urdf.xacro'),
        ' xyz:=', launch_params['odom2camera']['xyz'], ' rpy:=', launch_params['odom2camera']['rpy']])
    
    # 加载 robot_navigation_description
    robot_navigation_description = Command(['xacro ', os.path.join(
        robot_description_dir, 'urdf', 'sentry.urdf.xacro')])

    # 定义 robot_gimbal_publisher
    robot_gimbal_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_gimbal_description,
                    'publish_frequency': 1000.0}]
    )

    # 合并所有节点到 launch 描述中
    launch_description_list = [
        robot_gimbal_publisher
    ]
    
    return LaunchDescription(launch_description_list)